{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 01 查看模型\n",
    "\n",
    "首先加载和显示模型\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 参考manipulation课程中的示例，先加载一些默认的工具"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 加载Drake的主体"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator)\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 使用框图、平台和场景图建模，然后用meshcat显示机器人图形"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6000...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "9ad71d6e441140a6bd2efa7bbc90edf6",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_fl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a90238a8ee5a4770a43d607e31af1ee8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_fl_to_thigh_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1364c161e2ac42bbbf2b903aa4dda3c4",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_fl_to_knee_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f531c23b594f4675b669fada4d711ee2",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_fr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5f08ebbc4b05440b8822d02e9b84b478",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_fr_to_thigh_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "cf3c012a928e40faa3bb7bcfb7c51f02",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_fr_to_knee_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5681728c03f74699aead33b2d8e88845",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_hl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "07c52ee050164e118a609e372ce4794e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_hl_to_thigh_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0feee615e9684568a2d86479cfb525ce",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_hl_to_knee_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "13723a45118948bc881607b6a98fbd14",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='torso_to_abduct_hr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f0c82134506b49ebbb770e8034390c00",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='abduct_hr_to_thigh_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "2b4e99d7069e4097a61d1d339b6a43f7",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='thigh_hr_to_knee_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-3)\n",
    "\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"../models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "\n",
    "plant.Finalize()\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 将当前系统的结构以图形的方式显示（基于pydot和graphviz）"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"495pt\" viewBox=\"0.00 0.00 1154.00 495.00\" width=\"1154pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 491)\">\n",
       "<title>_94225761332096</title>\n",
       "<polygon fill=\"white\" points=\"-4,4 -4,-491 1150,-491 1150,4 -4,4\" stroke=\"transparent\"/>\n",
       "<g class=\"cluster\" id=\"clust1\">\n",
       "<title>cluster94225761332096diagram</title>\n",
       "<polygon fill=\"none\" points=\"8,-8 8,-479 1138,-479 1138,-8 8,-8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"573\" y=\"-463.8\">94225761332096</text>\n",
       "</g>\n",
       "<g class=\"cluster\" id=\"clust4\">\n",
       "<title>cluster94225761332096subsystems</title>\n",
       "<polygon fill=\"none\" points=\"16,-16 16,-448 1130,-448 1130,-16 16,-16\" stroke=\"white\"/>\n",
       "</g>\n",
       "<!-- 94225753818400 -->\n",
       "<g class=\"node\" id=\"node1\">\n",
       "<title>94225753818400</title>\n",
       "<polygon fill=\"none\" points=\"24,-25 24,-439 655,-439 655,-25 24,-25\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-423.8\">plant</text>\n",
       "<polyline fill=\"none\" points=\"24,-416 655,-416 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-379.8\">geometry_query</text>\n",
       "<polyline fill=\"none\" points=\"24,-351 275,-351 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-314.8\">WorldModelInstance_actuation</text>\n",
       "<polyline fill=\"none\" points=\"24,-286 275,-286 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-249.8\">DefaultModelInstance_actuation</text>\n",
       "<polyline fill=\"none\" points=\"24,-221 275,-221 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-184.8\">mini_cheetah_actuation</text>\n",
       "<polyline fill=\"none\" points=\"24,-156 275,-156 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-119.8\">applied_generalized_force</text>\n",
       "<polyline fill=\"none\" points=\"24,-91 275,-91 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-54.3\">applied_spatial_force</text>\n",
       "<polyline fill=\"none\" points=\"275,-25 275,-416 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-400.8\">geometry_pose</text>\n",
       "<polyline fill=\"none\" points=\"275,-393 655,-393 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-377.8\">continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-370 655,-370 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-354.8\">body_poses</text>\n",
       "<polyline fill=\"none\" points=\"275,-347 655,-347 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-331.8\">spatial_velocities</text>\n",
       "<polyline fill=\"none\" points=\"275,-324 655,-324 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-308.8\">spatial_accelerations</text>\n",
       "<polyline fill=\"none\" points=\"275,-301 655,-301 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-285.8\">generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-278 655,-278 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-262.8\">WorldModelInstance_continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-255 655,-255 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-239.8\">WorldModelInstance_generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-232 655,-232 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-216.8\">DefaultModelInstance_continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-209 655,-209 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-193.8\">DefaultModelInstance_generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-186 655,-186 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-170.8\">mini_cheetah_continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-163 655,-163 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-147.8\">mini_cheetah_generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-140 655,-140 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-124.8\">WorldModelInstance_generalized_contact_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-117 655,-117 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-101.8\">DefaultModelInstance_generalized_contact_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-94 655,-94 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-78.8\">mini_cheetah_generalized_contact_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-71 655,-71 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-55.8\">reaction_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-48 655,-48 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-32.8\">contact_results</text>\n",
       "</g>\n",
       "<!-- 94225737838112 -->\n",
       "<g class=\"node\" id=\"node2\">\n",
       "<title>94225737838112</title>\n",
       "<polygon fill=\"none\" points=\"691,-370.5 691,-439.5 925,-439.5 925,-370.5 691,-370.5\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"808\" y=\"-424.3\">scene_graph</text>\n",
       "<polyline fill=\"none\" points=\"691,-416.5 925,-416.5 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"738\" y=\"-389.8\">plant_pose</text>\n",
       "<polyline fill=\"none\" points=\"785,-370.5 785,-416.5 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"855\" y=\"-401.3\">lcm_visualization</text>\n",
       "<polyline fill=\"none\" points=\"785,-393.5 925,-393.5 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"855\" y=\"-378.3\">query</text>\n",
       "</g>\n",
       "<!-- 94225753818400&#45;&gt;94225737838112 -->\n",
       "<g class=\"edge\" id=\"edge1\">\n",
       "<title>94225753818400:y0-&gt;94225737838112:u0</title>\n",
       "<path d=\"M655,-405C667.78,-405 672.18,-398.11 680.9,-394.77\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"681.75,-398.17 691,-393 680.55,-391.28 681.75,-398.17\" stroke=\"black\"/>\n",
       "</g>\n",
       "<!-- 94225737838112&#45;&gt;94225753818400 -->\n",
       "<g class=\"edge\" id=\"edge3\">\n",
       "<title>94225737838112:y1-&gt;94225753818400:u0</title>\n",
       "<path d=\"M855,-369C855,-296.11 753.16,-331.93 691,-370 661.33,-388.17 684.72,-421.42 655,-439.5 595.05,-475.96 73.56,-488.67 23.5,-439.5 8.79,-425.05 0.73,-395.98 12.9,-386.82\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"13.89,-390.18 22.5,-384 11.92,-383.46 13.89,-390.18\" stroke=\"black\"/>\n",
       "</g>\n",
       "<!-- 94225743056848 -->\n",
       "<g class=\"node\" id=\"node3\">\n",
       "<title>94225743056848</title>\n",
       "<polygon fill=\"none\" points=\"961,-394 961,-440 1122,-440 1122,-394 961,-394\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1041.5\" y=\"-424.8\">meshcat_visualizer</text>\n",
       "<polyline fill=\"none\" points=\"961,-417 1122,-417 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1031\" y=\"-401.8\">lcm_visualization</text>\n",
       "<polyline fill=\"none\" points=\"1101,-394 1101,-417 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1111.5\" y=\"-401.8\"> </text>\n",
       "</g>\n",
       "<!-- 94225737838112&#45;&gt;94225743056848 -->\n",
       "<g class=\"edge\" id=\"edge2\">\n",
       "<title>94225737838112:y0-&gt;94225743056848:u0</title>\n",
       "<path d=\"M925,-405C937,-405 942.25,-405 950.88,-405\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"951,-408.5 961,-405 951,-401.5 951,-408.5\" stroke=\"black\"/>\n",
       "</g>\n",
       "</g>\n",
       "</svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 查看多体平台的模型结构"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"470pt\" viewBox=\"0.00 0.00 1275.00 470.00\" width=\"1275pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 466)\">\n",
       "<title>MultibodyPlant</title>\n",
       "<polygon fill=\"white\" points=\"-4,4 -4,-466 1271,-466 1271,4 -4,4\" stroke=\"transparent\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"633.5\" y=\"-446.8\">plant</text>\n",
       "<g class=\"cluster\" id=\"clust1\">\n",
       "<title>cluster0</title>\n",
       "<polygon fill=\"none\" points=\"8,-8 8,-83 173,-83 173,-8 8,-8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"90.5\" y=\"-15.8\">WorldModelInstance</text>\n",
       "</g>\n",
       "<g class=\"cluster\" id=\"clust3\">\n",
       "<title>cluster2</title>\n",
       "<polygon fill=\"none\" points=\"181,-8 181,-431 1259,-431 1259,-8 181,-8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"720\" y=\"-15.8\">mini_cheetah</text>\n",
       "</g>\n",
       "<!-- body0 -->\n",
       "<g class=\"node\" id=\"node1\">\n",
       "<title>body0</title>\n",
       "<ellipse cx=\"90\" cy=\"-57\" fill=\"none\" rx=\"61.99\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"90\" y=\"-53.3\">WorldBody</text>\n",
       "</g>\n",
       "<!-- body1 -->\n",
       "<g class=\"node\" id=\"node2\">\n",
       "<title>body1</title>\n",
       "<ellipse cx=\"623\" cy=\"-405\" fill=\"none\" rx=\"33.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"623\" y=\"-401.3\">body</text>\n",
       "</g>\n",
       "<!-- body2 -->\n",
       "<g class=\"node\" id=\"node3\">\n",
       "<title>body2</title>\n",
       "<ellipse cx=\"1007\" cy=\"-318\" fill=\"none\" rx=\"53.89\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1007\" y=\"-314.3\">abduct_fl</text>\n",
       "</g>\n",
       "<!-- body2&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge1\">\n",
       "<title>body2-&gt;body1</title>\n",
       "<path d=\"M996.32,-335.93C988.13,-347.33 975.76,-361.59 961,-369 910.35,-394.42 745.35,-401.4 666.17,-403.3\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"665.96,-399.8 656.04,-403.52 666.12,-406.8 665.96,-399.8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1090\" y=\"-357.8\">torso_to_abduct_fl_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body3 -->\n",
       "<g class=\"node\" id=\"node4\">\n",
       "<title>body3</title>\n",
       "<ellipse cx=\"1013\" cy=\"-231\" fill=\"none\" rx=\"46.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1013\" y=\"-227.3\">thigh_fl</text>\n",
       "</g>\n",
       "<!-- body3&#45;&gt;body2 -->\n",
       "<g class=\"edge\" id=\"edge2\">\n",
       "<title>body3-&gt;body2</title>\n",
       "<path d=\"M1011.79,-249.2C1010.96,-260.84 1009.86,-276.45 1008.92,-289.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1005.43,-289.6 1008.21,-299.82 1012.41,-290.1 1005.43,-289.6\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1129\" y=\"-270.8\">abduct_fl_to_thigh_fl_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body4 -->\n",
       "<g class=\"node\" id=\"node5\">\n",
       "<title>body4</title>\n",
       "<ellipse cx=\"1013\" cy=\"-144\" fill=\"none\" rx=\"50.09\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1013\" y=\"-140.3\">shank_fl</text>\n",
       "</g>\n",
       "<!-- body4&#45;&gt;body3 -->\n",
       "<g class=\"edge\" id=\"edge3\">\n",
       "<title>body4-&gt;body3</title>\n",
       "<path d=\"M1013,-162.2C1013,-173.84 1013,-189.45 1013,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1009.5,-202.82 1013,-212.82 1016.5,-202.82 1009.5,-202.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1124.5\" y=\"-183.8\">thigh_fl_to_knee_fl_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body5 -->\n",
       "<g class=\"node\" id=\"node6\">\n",
       "<title>body5</title>\n",
       "<ellipse cx=\"747\" cy=\"-318\" fill=\"none\" rx=\"55.79\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"747\" y=\"-314.3\">abduct_fr</text>\n",
       "</g>\n",
       "<!-- body5&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge4\">\n",
       "<title>body5-&gt;body1</title>\n",
       "<path d=\"M743.15,-336.38C739.94,-347.1 734.36,-360.4 725,-369 708.49,-384.18 685.11,-392.81 664.89,-397.71\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"663.98,-394.32 654.96,-399.88 665.48,-401.16 663.98,-394.32\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"846\" y=\"-357.8\">torso_to_abduct_fr_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body6 -->\n",
       "<g class=\"node\" id=\"node7\">\n",
       "<title>body6</title>\n",
       "<ellipse cx=\"748\" cy=\"-231\" fill=\"none\" rx=\"48.19\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"748\" y=\"-227.3\">thigh_fr</text>\n",
       "</g>\n",
       "<!-- body6&#45;&gt;body5 -->\n",
       "<g class=\"edge\" id=\"edge5\">\n",
       "<title>body6-&gt;body5</title>\n",
       "<path d=\"M747.8,-249.2C747.66,-260.84 747.48,-276.45 747.32,-289.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"743.82,-289.78 747.2,-299.82 750.82,-289.87 743.82,-289.78\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"868\" y=\"-270.8\">abduct_fr_to_thigh_fr_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body7 -->\n",
       "<g class=\"node\" id=\"node8\">\n",
       "<title>body7</title>\n",
       "<ellipse cx=\"749\" cy=\"-144\" fill=\"none\" rx=\"51.19\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"749\" y=\"-140.3\">shank_fr</text>\n",
       "</g>\n",
       "<!-- body7&#45;&gt;body6 -->\n",
       "<g class=\"edge\" id=\"edge6\">\n",
       "<title>body7-&gt;body6</title>\n",
       "<path d=\"M748.8,-162.2C748.66,-173.84 748.48,-189.45 748.32,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"744.82,-202.78 748.2,-212.82 751.82,-202.87 744.82,-202.78\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"861.5\" y=\"-183.8\">thigh_fr_to_knee_fr_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body8 -->\n",
       "<g class=\"node\" id=\"node9\">\n",
       "<title>body8</title>\n",
       "<ellipse cx=\"490\" cy=\"-318\" fill=\"none\" rx=\"56.59\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"490\" y=\"-314.3\">abduct_hl</text>\n",
       "</g>\n",
       "<!-- body8&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge7\">\n",
       "<title>body8-&gt;body1</title>\n",
       "<path d=\"M488.38,-336.25C488.34,-346.92 490.16,-360.21 498,-369 518.35,-391.82 552.24,-400.28 579.6,-403.2\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"579.46,-406.7 589.72,-404.06 580.06,-399.72 579.46,-406.7\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"609.5\" y=\"-357.8\">torso_to_abduct_hl_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body9 -->\n",
       "<g class=\"node\" id=\"node10\">\n",
       "<title>body9</title>\n",
       "<ellipse cx=\"488\" cy=\"-231\" fill=\"none\" rx=\"48.99\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"488\" y=\"-227.3\">thigh_hl</text>\n",
       "</g>\n",
       "<!-- body9&#45;&gt;body8 -->\n",
       "<g class=\"edge\" id=\"edge8\">\n",
       "<title>body9-&gt;body8</title>\n",
       "<path d=\"M488.4,-249.2C488.68,-260.84 489.05,-276.45 489.36,-289.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"485.86,-289.91 489.6,-299.82 492.86,-289.74 485.86,-289.91\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"611.5\" y=\"-270.8\">abduct_hl_to_thigh_hl_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body10 -->\n",
       "<g class=\"node\" id=\"node11\">\n",
       "<title>body10</title>\n",
       "<ellipse cx=\"496\" cy=\"-144\" fill=\"none\" rx=\"51.99\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"496\" y=\"-140.3\">shank_hl</text>\n",
       "</g>\n",
       "<!-- body10&#45;&gt;body9 -->\n",
       "<g class=\"edge\" id=\"edge9\">\n",
       "<title>body10-&gt;body9</title>\n",
       "<path d=\"M494.38,-162.2C493.29,-173.84 491.82,-189.45 490.56,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"487.07,-202.54 489.62,-212.82 494.04,-203.2 487.07,-202.54\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"607\" y=\"-183.8\">thigh_hl_to_knee_hl_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body11 -->\n",
       "<g class=\"node\" id=\"node12\">\n",
       "<title>body11</title>\n",
       "<ellipse cx=\"250\" cy=\"-318\" fill=\"none\" rx=\"57.69\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"250\" y=\"-314.3\">abduct_hr</text>\n",
       "</g>\n",
       "<!-- body11&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge10\">\n",
       "<title>body11-&gt;body1</title>\n",
       "<path d=\"M248.64,-336.34C248.8,-347.32 251.07,-360.93 260,-369 283.32,-390.07 488.92,-399.51 579.95,-402.69\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"579.83,-406.19 589.95,-403.03 580.07,-399.19 579.83,-406.19\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"373\" y=\"-357.8\">torso_to_abduct_hr_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body12 -->\n",
       "<g class=\"node\" id=\"node13\">\n",
       "<title>body12</title>\n",
       "<ellipse cx=\"239\" cy=\"-231\" fill=\"none\" rx=\"50.09\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"239\" y=\"-227.3\">thigh_hr</text>\n",
       "</g>\n",
       "<!-- body12&#45;&gt;body11 -->\n",
       "<g class=\"edge\" id=\"edge11\">\n",
       "<title>body12-&gt;body11</title>\n",
       "<path d=\"M234.63,-249.28C232.84,-258.98 231.66,-271.26 234,-282 234.63,-284.88 235.51,-287.8 236.55,-290.67\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"233.39,-292.2 240.49,-300.06 239.85,-289.49 233.39,-292.2\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"359\" y=\"-270.8\">abduct_hr_to_thigh_hr_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body13 -->\n",
       "<g class=\"node\" id=\"node14\">\n",
       "<title>body13</title>\n",
       "<ellipse cx=\"245\" cy=\"-144\" fill=\"none\" rx=\"53.89\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"245\" y=\"-140.3\">shank_hr</text>\n",
       "</g>\n",
       "<!-- body13&#45;&gt;body12 -->\n",
       "<g class=\"edge\" id=\"edge12\">\n",
       "<title>body13-&gt;body12</title>\n",
       "<path d=\"M241.49,-162.02C240.47,-167.71 239.5,-174.1 239,-180 238.38,-187.32 238.17,-195.27 238.16,-202.63\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"234.67,-202.89 238.28,-212.85 241.66,-202.8 234.67,-202.89\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"356.5\" y=\"-183.8\">thigh_hr_to_knee_hr_j [revolute]</text>\n",
       "</g>\n",
       "<!-- body14 -->\n",
       "<g class=\"node\" id=\"node15\">\n",
       "<title>body14</title>\n",
       "<ellipse cx=\"1013\" cy=\"-57\" fill=\"none\" rx=\"56.59\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1013\" y=\"-53.3\">LF_FOOT</text>\n",
       "</g>\n",
       "<!-- body14&#45;&gt;body4 -->\n",
       "<g class=\"edge\" id=\"edge13\">\n",
       "<title>body14-&gt;body4</title>\n",
       "<path d=\"M1013,-75.2C1013,-86.84 1013,-102.45 1013,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1009.5,-115.82 1013,-125.82 1016.5,-115.82 1009.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1105\" y=\"-96.8\">shank_fl_to_foot_fl [weld]</text>\n",
       "</g>\n",
       "<!-- body15 -->\n",
       "<g class=\"node\" id=\"node16\">\n",
       "<title>body15</title>\n",
       "<ellipse cx=\"749\" cy=\"-57\" fill=\"none\" rx=\"57.39\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"749\" y=\"-53.3\">RF_FOOT</text>\n",
       "</g>\n",
       "<!-- body15&#45;&gt;body7 -->\n",
       "<g class=\"edge\" id=\"edge14\">\n",
       "<title>body15-&gt;body7</title>\n",
       "<path d=\"M749,-75.2C749,-86.84 749,-102.45 749,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"745.5,-115.82 749,-125.82 752.5,-115.82 745.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"843\" y=\"-96.8\">shank_fr_to_foot_fr [weld]</text>\n",
       "</g>\n",
       "<!-- body16 -->\n",
       "<g class=\"node\" id=\"node17\">\n",
       "<title>body16</title>\n",
       "<ellipse cx=\"496\" cy=\"-57\" fill=\"none\" rx=\"58.49\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"496\" y=\"-53.3\">LH_FOOT</text>\n",
       "</g>\n",
       "<!-- body16&#45;&gt;body10 -->\n",
       "<g class=\"edge\" id=\"edge15\">\n",
       "<title>body16-&gt;body10</title>\n",
       "<path d=\"M496,-75.2C496,-86.84 496,-102.45 496,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"492.5,-115.82 496,-125.82 499.5,-115.82 492.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"591.5\" y=\"-96.8\">shank_hl_to_foot_hl [weld]</text>\n",
       "</g>\n",
       "<!-- body17 -->\n",
       "<g class=\"node\" id=\"node18\">\n",
       "<title>body17</title>\n",
       "<ellipse cx=\"248\" cy=\"-57\" fill=\"none\" rx=\"59.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"248\" y=\"-53.3\">RH_FOOT</text>\n",
       "</g>\n",
       "<!-- body17&#45;&gt;body13 -->\n",
       "<g class=\"edge\" id=\"edge16\">\n",
       "<title>body17-&gt;body13</title>\n",
       "<path d=\"M247.39,-75.2C246.98,-86.84 246.43,-102.45 245.96,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"242.46,-115.71 245.61,-125.82 249.46,-115.95 242.46,-115.71\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"344\" y=\"-96.8\">shank_hr_to_foot_hr [weld]</text>\n",
       "</g>\n",
       "</g>\n",
       "</svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "SVG(pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].create_svg())"
   ]
  },
  {
   "source": [
    "## 现象\n",
    "\n",
    "模型导入正常。\n",
    "三维图像显示正常。\n"
   ],
   "cell_type": "markdown",
   "metadata": {}
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}